#ifndef MAPDRAWER_H
#define MAPDRAWER_H

#include "Atlas.h"
#include "KeyPoseFrame.h"
#include "Settings.h"
#include<pangolin/pangolin.h>

#include<mutex>

namespace msf
{
class Settings;
class Atlas;

class MapDrawer
{
public:
      EIGEN_MAKE_ALIGNED_OPERATOR_NEW
      MapDrawer(Atlas* pAtlas, Settings* settings);

      void newParameterLoader(Settings* settings);

      Atlas* mpAtlas;

      void DrawKeyPoseFrames(const bool bDrawKPF, const bool bDrawInertialGraph);
      void DrawCurrentOdom(pangolin::OpenGlMatrix &Two);
      void SetCurrentOdomPose(const Sophus::SE3f &Tow);
      void SetReferenceKeyFrame(KeyPoseFrame *pKPF);
      void GetCurrentOpenGLOdomMatrix(pangolin::OpenGlMatrix &M, pangolin::OpenGlMatrix &MOw);

private:

      float mKeyPoseFrameSize;
      float mKeyPoseFrameLineWidth;
      float mGraphLineWidth;
      float mPointSize;
      float mOdomSize;
      float mOdomLineWidth;

      Sophus::SE3f mOdomPose;

      std::mutex mMutexOdom;

      float mfFrameColors[6][3] = {{0.0f, 0.0f, 1.0f},
                                    {0.8f, 0.4f, 1.0f},
                                    {1.0f, 0.2f, 0.4f},
                                    {0.6f, 0.0f, 1.0f},
                                    {1.0f, 1.0f, 0.0f},
                                    {0.0f, 1.0f, 1.0f}};

      Eigen::Matrix4f rotate;
};

} //namespace msf

#endif //MAPDRAWER_H